#include "app_task.h"
#include "cmsis_os.h"
#include "bsp_imu.h"
#include "mahony_ahrs.h"
#include "control.h"
#include "hostComputer.h"
#include "filter.h"
#include "bsp_c610.h"
extern pid_t attitude_outer_pid;
extern imu_t imu;
extern control_info_t control_info;

osThreadId_t control_task_handler;
const osThreadAttr_t control_task_attributes = {
  .name = "control task",
  .stack_size = 256 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};
void control_task(void *argument)
{
	float yn_1 = 0;
	float xn_1 = 0,xn = 0,yn = 0;
	float left_mileage = 0,right_mileage = 0;
	for(;;)
  {
		mpu_get_data();
		//get mpu data
		imu_ahrs_update();
		//attitude solution

		left_mileage = c610_get_mileage( LEFT_MOTOR_ID );
		right_mileage = c610_get_mileage( RIGHT_MOTOR_ID );			
		update_car_mileage( right_mileage,left_mileage );
		//unit: round cnt * Wheel circumference = m/s;
		//motor mileage update
			
		
		xn = xn_1;
		xn_1 = imu.wy;
		yn = yn_1;
		yn_1 = LowpassFilter(0.002f,4.f,yn,xn_1,xn);
		imu.lpf_wy = yn_1;

		
		attitude_control(0.002f);
		
		osDelay(2);
	}
}

osThreadId_t system_task_handler;
const osThreadAttr_t system_task_attributes = {
  .name = "system task",
  .stack_size = 128 * 4,
  .priority = (osPriority_t) osPriorityNormal,
};

void system_task(void *argument)
{

	float transmit[20] = {0};
	for(;;)
	{		
		transmit[0] = imu.pit;
		transmit[1] = MECHANICAL_MEDIAN_ANGLE;
		transmit[2] = -attitude_outer_pid.output / 180.f * PI;		
		transmit[3] = imu.lpf_wy;
		transmit[4] = control_info.motor_speed_ref;
		transmit[5] = c610_get_speed(LEFT_MOTOR_ID);
		HostComputer_SendUserData_float(FUNC_ID1,transmit,6);
		
		
//		HAL_GPIO_TogglePin(LED_G_GPIO_Port,LED_G_Pin);
//		HAL_GPIO_TogglePin(LED_R_GPIO_Port,LED_R_Pin);
		osDelay(50);
	}
}

void task_init(void)
{
	control_task_handler = osThreadNew(control_task, NULL, &control_task_attributes);
	system_task_handler = osThreadNew(system_task, NULL, &system_task_attributes);
}



